;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; FILE:   blocks.lisp
;; AUTHOR: Chris MacLellan - modified RAB
;; DESC:   icarus concepts and skills for ex6 in blocks world
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; Exercise 6 - Blocks World (Task A)

(create-concepts
 ;; perceptual typing concepts
 ((block ?block)
  :percepts    ((block ?block)))

 ((table ?table)
  :percepts    ((table ?table)))

 ;; other concepts
 ((on ?block1 ?block2)
  :percepts    ((block ?block1 xpos ?xpos1 ypos ?ypos1)
		(block ?block2 xpos ?xpos1 ypos ?ypos2 height ?height2))
  :tests       ((= ?ypos1 (+ ?ypos2 ?height2))))

 ((on-table ?block ?table)
  :percepts    ((block ?block xpos ?xpos1 ypos ?ypos1)
		(table ?table xpos ?xpos2 ypos ?ypos2 height ?height2))
  :tests       ((>= ?ypos1 ?ypos2) (<= ?ypos1 (+ ?ypos2 ?height2))))

 ((holding ?block)
  :percepts    ((block ?block)
		(hand ?hand status ?block)))

 ((distinct ?block1 ?block2)
  :percepts    ((block ?block1)
		(block ?block2))
  :tests       ((not (equal ?block1 ?block2))
		(string< ?block1 ?block2)))

 ((two-tower ?block1 ?block2)
  :percepts     ((block ?block1)
		 (block ?block2))
  :relations    ((on ?block1 ?block2) (on-table ?block2 ?table1)))

 ((two-stack ?block1 ?block2)
  :percepts     ((block ?block1)
		 (block ?block2))
  :relations    ((on ?block1 ?block2)))

;; ((tower ?block1 ?block2)
;;  :percepts     ((block ?block1)
;;		 (block ?block2))
;;  
;;  :relations    ((on ?block1 ?other-block) (tower ?other-block ?block2)))

)

(create-skills

 ;; unstack
 ((unstack ?block ?block2)
  :percepts    ((block ?block)
		(block ?block2))
  :conditions  ((on ?block ?block2)
;		(distinct ?block ?block2)
		(not (on ?any2 ?block))
		(not (holding ?any3)))
  :action      (*grasp-and-raise ?block)
  :effects     ((holding ?block)
		(not (on ?block ?block2))))
  
 ;; pickup
 ((pick-up ?block)
  :percepts    ((block ?block)
		(table ?table))
  :conditions  ((on-table ?block ?table)
		(not (on ?any ?block))
		(not (holding ?any2)))
  :action      (*grasp-and-raise ?block)
  :effects     ((holding ?block)
		(not (on-table ?block ?table))))


 ((stack ?block1 ?to)
  :percepts    ((block ?block1)
		(block ?to))
  :conditions  ((holding ?block1)
;		(distinct ?block1 ?to)
		(not (on ?any ?to)))
  :action      (*place-and-ungrasp ?block1 ?to)
  :effects     ((not (holding ?block1))
		(on ?block1 ?to)))

 ;; putdown 
 ((put-down ?block)
  :percepts    ((block ?block)
		(table ?table))
  :conditions  ((holding ?block))
  :action      (*place-and-ungrasp ?block ?table)
  :effects     ((not (holding ?block))
		(on-table ?block ?table)))

;; pick-up-and-stack
#+WE-CAN-BUILD-THIS
 ((pick-up-and-stack ?block1 ?block2)
  :percepts    ((block ?block1) (block ?block2)
		(table ?table))
  :conditions  ((on-table ?block1 ?table) ; has to be for pickup to work
		(distinct ?block1 ?block2)
		(not (on ?any ?block1))
		(not (on ?any ?block2))
		(not (holding ?any2)))
  :subskills   ((pick-up ?block1) (stack ?block1 ?block2))
  :effects     ((not (on-table ?block1 ?table))
		(on ?block1 ?block2)))

#+WE-CAN-BUILD-THIS
((unstack-and-stack ?block1 ?block2)
  :percepts    ((block ?block1) (block ?block2) (block ?anyblock) (table ?table))
  :conditions  ((distinct ?block1 ?block2)
		(on ?block1 ?anyblock)
		(not (on ?any ?block1))
		(not (on ?any1 ?block2))
		(not (holding ?any2)))
  :subskills   ((unstack ?block1) (stack ?block1 ?block2))
  :effects     ((on ?block1 ?block2)
		(not (on ?block1 ?anyblock))))
)
